#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <cmath>

#include "nav_msgs/Odometry.h"

float xx, yy, zz;

// visualization_msgs/Marker消息，并且初始化所有共享的数据。消息成员默认为0，仅仅设置位姿成员w。
visualization_msgs::Marker points, line_strip, line_list;

void doPose(const nav_msgs::Odometry::ConstPtr& p) {
  xx = p->pose.pose.position.x;
  yy = p->pose.pose.position.y;
  zz = p->pose.pose.position.z;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "points_and_lines");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe<nav_msgs::Odometry>("/odom", 1000, doPose);
  ros::Publisher marker_pub =
      n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Rate r(30);
  float f = 0.0;
  points.header.frame_id = line_strip.header.frame_id =
      line_list.header.frame_id = "/my_frame";
  points.header.stamp = line_strip.header.stamp = line_list.header.stamp =
      ros::Time::now();
  points.ns = line_strip.ns = line_list.ns = "points_and_lines";
  points.action = line_strip.action = line_list.action =
      visualization_msgs::Marker::ADD;
  points.pose.orientation.w = line_strip.pose.orientation.w =
      line_list.pose.orientation.w = 1.0;
  //分配三个不同的id到三个markers。points_and_lines名称空间的使用确保彼此不会相互冲突。
  points.id = 0;
  line_strip.id = 1;
  line_list.id = 2;
  //设置marker类型到 POINTS, LINE_STRIP 和 LINE_LIST
  points.type = visualization_msgs::Marker::POINTS;
  // line_strip.type = visualization_msgs::Marker::LINE_STRIP;
  // line_list.type = visualization_msgs::Marker::LINE_LIST;
  // scale成员对于这些marker类型是不同的,POINTS
  // marker分别使用x和y作为宽和高，然而LINE_STRIP和LINE_LIST
  // marker仅仅使用x，定义为线的宽度。单位是米。
  points.scale.x = 0.1;
  points.scale.y = 0.1;
  // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the
  // line width
  // line_strip.scale.x = 0.1;
  // line_list.scale.x = 0.1;
  // 点为绿色
  points.color.g = 1.0f;
  points.color.a = 1.0;
  // Line strip 是蓝色
  // line_strip.color.b = 1.0;
  // line_strip.color.a = 1.0;
  // Line list 为红色
  // line_list.color.r = 1.0;
  // line_list.color.a = 1.0;
  while (ros::ok()) {
    ros::spinOnce();
    geometry_msgs::Point p;
    p.x = xx;
    p.y = yy;
    p.z = zz;
    points.points.push_back(p);
    // line_strip.points.push_back(p);
    // The line list needs two points for each line
    // line_list.points.push_back(p);
    // p.z += 1.0;
    // line_list.points.push_back(p);
    //发布各个markers
    marker_pub.publish(points);
    // marker_pub.publish(line_strip);
    // marker_pub.publish(line_list);
    r.sleep();
    f += 0.04;
  }
}
